% Reference:
% 1. Impact-Time-Control Guidance Law for Anti-Ship Missiles 
% 2. Multiple Missiles Cooperative Guidance Based on PNG
% 二维PNG ITCG
clear
clc
close all
% method = 'PNG';
method = 'ITCG';
deg2rad = pi/180;
V0 = 300;
lambda = nan;
condition = 0;
switch condition
    case 0
        X0 = 0;Y0 = 0;
        XT = 1e4;YT = 0;
        theta0 = 30*deg2rad;
    case 1
        X0 = -6364;Y0 = -6364;
        XT = 0;YT = 0;
        theta0 = 75*deg2rad;    
end
TD = 38;
step = 0.1;
k = TD/step;
init = [X0;Y0;theta0];
y = init;acc = nan; epsilonT = nan;
res = nan(k,7);
for i = 1:k 
    res(i,1) = (i-1)*step;
    res(i,2:4) = y;
    res(i,5) = acc;
    res(i,6) = lambda;
    res(i,7) = epsilonT;
    [acc,lambda,epsilonT,y] = solver(y,method,XT,YT);
    if (y(2)- YT)^2+(y(1)- XT)^2 < 200
        k = i;
        break;
    end
end
%% 提取数据 + 作图
t = res(1:k,1);
x = res(1:k,2);
y = res(1:k,3);
theta = res(1:k,4)/deg2rad;
acc = res(1:k,5);
lambda = res(1:k,6);
epsilonT = res(1:k,7);

figure
plot(x,y,'LineWidth',2);
% ylim([0 5000])
xlabel('Downrange(m)');ylabel('Crossrange(m)');

figure
plot(t,theta,'LineWidth',2);
% ylim([-80 40]);
xlabel('Time(sec)');ylabel('Heading Angle(deg)');

figure
plot(t(3:k),acc(3:k),'LineWidth',2);
xlabel('Time(sec)');ylabel('acceleration');

figure
plot(t,epsilonT,'LineWidth',2);
xlabel('Time(sec)');ylabel('epsilonT');